#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>

#include <opencv2/opencv.hpp>

#include "flashai/commons/string_utils.h"
#include "flashros/commons/utils.h"

void CameraSubscribeCallback0(const sensor_msgs::ImageConstPtr& msg) {
  try {
    cv::imshow("0", cv_bridge::toCvShare(msg, "bgr8")->image);
    cv::waitKey(1);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

void CameraSubscribeCallback1(const sensor_msgs::ImageConstPtr& msg) {
  try {
    cv::imshow("1", cv_bridge::toCvShare(msg, "bgr8")->image);
    cv::waitKey(1);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char** argv) {
  // ros init
  flashros::commons::RosInit(argc, argv, "FlashROS_two_stream_subscriber");
  ros::NodeHandle nh;
  ROS_WARN("Successfully init ros for two stream subscirber");

  std::string config_param_name = "/two_stream_publisher/config_filename";
  std::string output_section = "output";

  // Read config file
  // 固定参数，设置配置文件路径
  INIReader* reader = flashros::commons::InitConfig(nh, config_param_name);
  ROS_WARN("Successfully read config file");

  std::string topic_names_str =
      reader->GetString(output_section, "topic_names", "");
  std::vector<std::string> topic_names;
  flashai::utils::SplitStr(topic_names_str, topic_names);
  ROS_WARN_STREAM("Successfully read topic names " << topic_names_str);

  // create subscribe and run
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub0 =
      it.subscribe(topic_names[0], 1, CameraSubscribeCallback0);
  image_transport::Subscriber sub1 =
      it.subscribe(topic_names[1], 1, CameraSubscribeCallback1);
  ROS_WARN_STREAM("Successfully init all subscribers");

  ros::spin();
  cv::destroyAllWindows();
}